#include "robot_locomotion.h"

using namespace std;

#define LD 31
#define RD 31
#define HD 14


rl::rl(void)
{
    //变量初始化
    cout << "初始化开始" << endl;
    state = 0;
    count = 0;
    vxctl = 0;
    del_dx = 0;
    del_dy = 0;
    del_yaw = 0;
    vx = 0;
    vy = 0;
    w = 0;
    kx = 0.05;
    ky = 0.05;
    kw = 0.5;
    dsensor[0] = 0;
    dsensor[1] = 0;
    dsensor[2] = 0;
    dsensor[3] = 0;
    dsensor[4] = 0;
    dsensor[5] = 0;
 

}
void rl::getvalue()
{
    dsensor[0] = 0;
    dsensor[1] = 0;
    dsensor[2] = 0;
    dsensor[3] = 0;
    dsensor[4] = 0;
    dsensor[5] = 0;

}

void rl::run()
{   
    //用于速度控制
    
    switch (state)
    {   
        case 0:
            vx = 0;
            vy = 0;
            w = 0;
            break;

        //直行
        case 1:
            cout << "case1 1号舱道直行" << endl;
            //前进方向速度
            vx = 0.05;
            if (dsensor[2] + dsensor[3] > 6)
            {
                vx = 0.02;
            }
            
            //侧向保持边距
            del_dy = (dsensor[0] + dsensor[1])/2 - 2.0;
            vy = ky * del_dy;

            //保持机身向前
            del_yaw = - atan2(dsensor[1] - dsensor[0],LD);
            w = kw * del_yaw;
            break;
        
        //原地转向
        case 2:
            cout << "case2 1号舱道原地转向" << endl;
            //90度原地转向
            vx = -0.02;
            vy = 0.015;
            w = -0.06;
            break;
        
        //对孔
        case 3:
            cout << "case3 1号舱道对孔" << endl;
            //保持前后间距
            del_dx = - ((dsensor[2] + dsensor[3])/2 - 1.25);
            vx = 0.7 * kx * del_dx;
            //vx  =  0;
            cout << "vx in cross" << vx << endl;
            //侧向保持边距
            del_dy = (dsensor[0] + dsensor[1])/2 - 2.6; 
            vy = 0.7 * ky * del_dy;

            //保持机身向前
            del_yaw = - atan2(dsensor[1] - dsensor[0],LD);
            w = kw * del_yaw;
            break;

        case 4:
            cout << "case4 1号舱道越障" << endl;
            //前进
            // if (vx < 0.04)
            // {
            //     vxctl++;
            // }
            // else
            // {
            //     vxctl--;
            // }
            vx = 0.15;
            
            //侧向保持边距
            if ((dsensor[0] > 2) && (dsensor[1] > 2))
            {
                del_dy = (dsensor[0] + dsensor[1])/2 - 2.6; 
                vy = ky * del_dy;
            }
            else
            {
                vy = 0;
            }
            
            //保持机身向前
             if ((dsensor[0] > 2) && (dsensor[1] > 2))
            {
                del_yaw = - atan2(dsensor[1] - dsensor[0],LD);
                w = kw * del_yaw;
            }
            else
            {
                w = 0;
            }
            break;
        
        //稳姿
        case 5:
            cout << "case5 2号舱道稳姿" << endl;
            //保持前后间距
            del_dx = - ((dsensor[2] + dsensor[3])/2 - 7);
            vx = 0.7 * kx * del_dx;

            //侧向保持边距
            del_dy = (dsensor[0] + dsensor[1])/2 - 2.6; 
            vy = 0.7 * ky * del_dy;

            //保持机身向前
            del_yaw = - atan2(dsensor[1] - dsensor[0],LD);
            w = kw * del_yaw;
            break;

        case 6:
            cout << "case5 2号舱道原地转向" << endl;
            //90度原地转向
             if (vx < 0)
             {
                 vxctl++;
             }
            vx = -0.02;
            vy = 0.015;
            w = - 0.06;
            break;

        case 7:
            cout << "case6 2号舱道直行" << endl;
            //前进方向速度
            vx = 0.1;
            
            //侧向保持边距
            del_dy = (dsensor[0] + dsensor[1])/2 - 2.6;
            vy = 0.3 * ky * del_dy;

            //保持机身向前
            del_yaw = - atan2(dsensor[1] - dsensor[0],LD);
            w = kw * del_yaw;
            break;
        
        case 8:
            cout << "hold..." << endl;

            del_dx = - ((dsensor[2] + dsensor[3])/2 - 15);
            vx = 0.3 * kx * del_dx;
            vy = 0;
            w = 0;
            break;

        

            

    }
    

}

void rl::update()
{
    switch (state)
    {
        //触发条件
        case 0:
            count++;
            if (count > 500)
            {
                count = 0;
                state = 1;
            }
            break;

        case 1:
            //连续检测到孔位，超过阈值次数达到5次
            if (dsensor[5] > 6)
            {
                count++;
            }

            if (count > 300)
            {
                count = 0;
                state = 2;
            }
            break;
        
        case 2:
            //后端检测距离连续小于阈值超过三次
            if ((dsensor[2] + dsensor[3] < 3.4) && (abs(dsensor[2] - dsensor[3]) < 0.02))
            {
                count++;
            }
            
            if (count > 5)
            {
                count = 0;
                state = 3;
            }
            break;
        
        case 3:
            //检测连续到位次数
             if ((abs(atan2(dsensor[1] - dsensor[0],LD)) < 0.01) && (abs((dsensor[0] + dsensor[1])/2 - 2.6) < 0.06))
            {
                count++;
            }
            
            if (count > 1)
            {
                count = 0;
                state = 4;
            }
            break;
        
        case 4:
            //越障状态
            cout << "越障步态" << endl;

            if ((dsensor[2] + dsensor[3]) > 14)
            {
                count++;
            }
            
            if (count > 5)
            {
                count = 0;
                state = 5;
            }
            break;

        case 5:
            if ((abs(atan2(dsensor[1] - dsensor[0],LD)) < 0.01) && (abs((dsensor[0] + dsensor[1])/2 - 2.6) < 0.03))
            {
                count++;
            }
            
            if (count > 200)
            {
                count = 0;
                state = 6;
            }
            break;

        case 6:
            //后端检测距离连续小于阈值
            if ((dsensor[2] + dsensor[3] < 4.8) && (abs(dsensor[2] - dsensor[3]) < 0.03))
            {
                count++;
            }
            
            if (count > 3)
            {
                count = 0;
                state = 7;
            }
            break;

        case 7:
            if ((dsensor[2] + dsensor[3]) > 20)
            {
                state = 8; 
            }
            break;
    }
}

void rl::sendvalue()
{
    // velx = vx;
    // vely = vy;
    // velw = w;
}